
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       fms_state.c
  * @author     baiyang
  * @date       2021-8-24
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "fms.h"

#include <uITC/uITC.h>
#include <uITC/uITC_msg.h>

#include <notify/notify.h>
#include <ahrs/ahrs_view.h>
#include <common/time/gp_time.h>
#include <common/geo/declination.h>
#include <board_config/borad_config.h>
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/
static MAV_TYPE get_frame_mav_type();
static MAV_MODE get_base_mode();
static MAV_STATE get_vehicle_system_status();
/*----------------------------------variable----------------------------------*/
static uitc_actuator_armed actuator_armed;
static uitc_vehicle_hearbeat vehicle_hearbeat;
static uitc_vehicle_vfr_hud vehicle_vfr_hud;
/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
void fms_set_auto_armed(bool b)
{
    // if no change, exit immediately
    if( fms.ap.auto_armed == b )
        return;

    fms.ap.auto_armed = b;
    if(b){
        //AP::logger().Write_Event(LogEvent::AUTO_ARMED);
    }
}

/**
  * @brief       
  * @param[in]   b  
  * @param[out]  
  * @retval      
  * @note        
  */
void fms_actuator_armed_notify(bool b)
{
    uint64_t timestamp_us = time_micros64();

    notify_flags.armed = b;

    brd_set_soft_armed(b);

    if (b && !ahrs_home_is_locked(fms.ahrs)) {
        fms_set_home_to_current_location(false);
    }

    actuator_armed.timestamp_us = timestamp_us;
    actuator_armed.armed = b;
    itc_publish(ITC_ID(vehicle_actuator_armed), &actuator_armed);
}

void fms_update_using_interlock()
{
    // check if we are using motor interlock control on an aux switch or are in throw mode
    // which uses the interlock to stop motors while the copter is being thrown
    fms.ap.using_interlock = rcs_find_channel_for_option(RC_AUX_MOTOR_INTERLOCK) != NULL;
}

// ---------------------------------------------
void fms_set_failsafe_radio(bool b)
{
    // only act on changes
    // -------------------
    if(fms.failsafe.radio != b) {

        // store the value so we don't trip the gate twice
        // -----------------------------------------------
        fms.failsafe.radio = b;

        if (fms.failsafe.radio == false) {
            // We've regained radio contact
            // ----------------------------
            fms_failsafe_radio_off_event();
        }else{
            // We've lost radio contact
            // ------------------------
            fms_failsafe_radio_on_event();
        }

        // update AP_Notify
        notify_flags.failsafe_radio = b;
    }
}


// ---------------------------------------------
void fms_set_failsafe_gcs(bool b)
{
    fms.failsafe.gcs = b;

    // update AP_Notify
    notify_flags.failsafe_gcs = b;
}

void fms_publish_vehicle_hearbeat()
{
    vehicle_hearbeat.timestamp_us = time_micros64();

    vehicle_hearbeat.custom_mode = (uint32_t)mode_number(fms.flightmode);
    vehicle_hearbeat.type = get_frame_mav_type();
    vehicle_hearbeat.autopilot = MAV_AUTOPILOT_ARDUPILOTMEGA;
    vehicle_hearbeat.base_mode = get_base_mode();
    vehicle_hearbeat.system_status = get_vehicle_system_status();

    itc_publish(ITC_ID(vehicle_hearbeat), &vehicle_hearbeat);
}

void fms_publish_vehicle_vfr_hud()
{
    vehicle_vfr_hud.timestamp_us = time_micros64();

    uitc_vehicle_position pos_info;
    itc_copy_from_hub(ITC_ID(vehicle_position), &pos_info);

    uitc_vehicle_alt alt_info;
    itc_copy_from_hub(ITC_ID(vehicle_alt), &alt_info);

    uitc_vehicle_attitude vehicle_attitude;
    itc_copy_from_hub(ITC_ID(vehicle_attitude), &vehicle_attitude);

    if (fms.motors != NULL) {
        vehicle_vfr_hud.throttle = Motors_get_throttle(fms.motors) * 100.0f;
    }

    vehicle_vfr_hud.airspeed = 0;
    vehicle_vfr_hud.groundspeed = sqrtf(pos_info.vx*pos_info.vx+pos_info.vy*pos_info.vy);
    vehicle_vfr_hud.alt      = alt_info.relative_alt;
    vehicle_vfr_hud.climb    = alt_info.vz;
    vehicle_vfr_hud.heading = math_wrap_360(vehicle_attitude.vehicle_euler.yaw*RAD_TO_DEG);

    itc_publish(ITC_ID(vehicle_vfr_hud), &vehicle_vfr_hud);
}

static MAV_TYPE get_frame_mav_type()
{
    switch ((motor_frame_class)fms.frame_class) {
        case MOTOR_FRAME_QUAD:
        case MOTOR_FRAME_UNDEFINED:
            return MAV_TYPE_QUADROTOR;
        case MOTOR_FRAME_HEXA:
        case MOTOR_FRAME_Y6:
            return MAV_TYPE_HEXAROTOR;
        case MOTOR_FRAME_OCTA:
        case MOTOR_FRAME_OCTAQUAD:
            return MAV_TYPE_OCTOROTOR;
        case MOTOR_FRAME_HELI:
        case MOTOR_FRAME_HELI_DUAL:
        case MOTOR_FRAME_HELI_QUAD:
            return MAV_TYPE_HELICOPTER;
        case MOTOR_FRAME_TRI:
            return MAV_TYPE_TRICOPTER;
        case MOTOR_FRAME_SINGLE:
        case MOTOR_FRAME_COAX:
        case MOTOR_FRAME_TAILSITTER:
            return MAV_TYPE_COAXIAL;
        case MOTOR_FRAME_DODECAHEXA:
            return MAV_TYPE_DODECAROTOR;
        case MOTOR_FRAME_DECA:
            return MAV_TYPE_DECAROTOR;
    }
    // unknown frame so return generic
    return MAV_TYPE_GENERIC;
}

static MAV_MODE get_base_mode()
{
    Motors_HandleTypeDef *motors = fms.motors;

    uint8_t _base_mode = MAV_MODE_FLAG_STABILIZE_ENABLED;
    // work out the base_mode. This value is not very useful
    // for APM, but we calculate it as best we can so a generic
    // MAVLink enabled ground station can work out something about
    // what the MAV is up to. The actual bit values are highly
    // ambiguous for most of the APM flight modes. In practice, you
    // only get useful information from the custom_mode, which maps to
    // the APM flight mode and has a well defined meaning in the
    // ArduPlane documentation
    switch (mode_number(fms.flightmode)) {
    case AUTO:
    case AUTO_RTL:
    case RTL:
    case LOITER:
    case AVOID_ADSB:
    case FOLLOW:
    case GUIDED:
    case CIRCLE:
    case POSHOLD:
    case BRAKE:
    case SMART_RTL:
        _base_mode |= MAV_MODE_FLAG_GUIDED_ENABLED;
        // note that MAV_MODE_FLAG_AUTO_ENABLED does not match what
        // APM does in any mode, as that is defined as "system finds its own goal
        // positions", which APM does not currently do
        break;
    default:
        break;
    }

    // all modes except INITIALISING have some form of manual
    // override if stick mixing is enabled
    _base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;

    // we are armed if we are not initialising
    if (motors != NULL && motors->_armed) {
        _base_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
    }

    // indicate we have set a custom mode
    _base_mode |= MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;

    return (MAV_MODE)_base_mode;
}

static MAV_STATE get_vehicle_system_status()
{
    // set system as critical if any failsafe have triggered
    if (fms.failsafe.radio || 
        fms.failsafe.gcs || 
        fms.failsafe.ekf || 
        fms.failsafe.terrain || 
        fms.failsafe.adsb)  {
        return MAV_STATE_CRITICAL;
    }

    if (fms.ap.land_complete) {
        return MAV_STATE_STANDBY;
    }

    return MAV_STATE_ACTIVE;
}

/*------------------------------------test------------------------------------*/


